﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Text;

namespace ESP
{
    public class EspAlgorithm
    {
        private const int FEEDFORWARD = 0;
        private const int SIMPLERECURRENT = 1;
        private const int SECONDORDER = 2;
        private const int FULLYRECURRENT = 3;

        //	public static int NUM_IN;          	// number of input from environment
        //	public static int NUM_INPUTS;      	// number of input units; includes recurrent input
        //	public static int NUM_OUTPUTS;     	// number of output units 
        //	private static int GENE_SIZE;        	// size is chromosomes
        private static int maxGenerations = 100;
        private static bool SKIP; // skip recombination;
        private static double MUT_RATE = 0.4; // mutation rate in %
        private static int STAG = 5; // the length of the performance queue.
        private static bool PLOTTING;
        private static bool DELTA;
        public static bool MIN = false;
        private static bool analyze;

        private readonly List<double> perfQ = new List<double>(); // double

        public Network bestNetwork; //make private after debug
        private IEnvironment environ;
        private int evaluations;
        private int generation;
       
        //  espParams params;
        public int netType;
        public int numTrials;
        public Network phaseBest;
      
        public long seed;
        public int seedNet;
        public int subPopulationCount; // The number of subpopulations in the gene pool
        public int subPopulationSize;

        public SubPopulation[] subpops; // SubPopulation
        // neurons 
        // constructor

        public Action<string> Logger { get; set; }

        private bool finished;

        private Network currentBest;


        public void init(IEnvironment e)
        {
            environ = e;
            generation = 0; // start at generation 0
            numTrials = subPopulationSize*10; // # of trials ~10/neuron

            bestNetwork = genNetType(); // "              "
            bestNetwork.create();

            phaseBest = genNetType(); // generate new net 
            phaseBest.create(); // create net  

            subpops = new SubPopulation[subPopulationCount];
            for (int i = 0; i < subpops.Length; ++i)
            {
                // construct subPopulationCount # of subPops
                subpops[i] = new SubPopulation(subPopulationSize, bestNetwork);
            }
        }

        //----------------------------------------------------------------------
        // create the subpopulations of neurons, initializing them to 
        //   random weights
        public void create()
        {
            // creates a random population of sub-populations
            for (int i = 0; i < subpops.Length; ++i)
            {
                subpops[i].create(); // create each subpop
            }
        }

        //////////////////////////////////////////////////////////////////////
        //
        //  evolve is the main genetic function.  The subpopulations are first
        //  evaluated in the given task.  Then for each subpop the neurons are
        //  ranked (sorted by fitness) and recombined using crossover.  
        //  Mating is allowed only between the top 'numBreed' members of 
        //  the same subpop. The neurons are then mutated.
        //
        //////////////////////////////////////////////////////////////////////
        // cycles: the number of generations to evolve
        public Network evolve(int cycles)
        {
            while ((SignalHandler.gInterrupt == false) && (++generation < cycles) && !finished)
            {
                evalPop(); //evaluate neurons
                if (SKIP)
                {
                    SKIP = false; // skip recombination if we have just 
                }
                else
                {
                    //    perturb the population
                    for (int j = 0; j < subpops.Length; ++j)
                    {
                        subpops[j].qsortNeurons();
                        //      	for(int k = 0; k < subpops[j].numNeurons; ++k) 
                        //      		System.out.print("%f ", subpops[j].getNeuron(k).fitness);
                        //      	Console.WriteLine();
                        subpops[j].recombine();
                    }

                    //printNeurons();

                    // mutate population 
                    for (int j = 0; j < subpops.Length; ++j)
                    {
                        subpops[j].mutate(MUT_RATE);
                    }
                }

                //if (environ.validate(currentBest))
                //{
                //    break;
                //}

            }
            endEvolution();

            return currentBest;
        }

        //////////////////////////////////////////////////////////////////////
        //
        //  eval_pop evaluates entire population to get the average fitness of
        //  each neuron.  This function is called by evolve and should put the
        //  fitness of each neuron in the neuron's fitness field
        //  (getNeuron(i).fitness).
        //
        //////////////////////////////////////////////////////////////////////
        public void evalPop()
        {
            evalReset(); // reset fitness and test values of each neuron
            performEval(); // evaluate the networks
            average(); // normalize neuron fitnesses
        }

        public void findChampion()
        {
            //double fitness, best=-999999;
            //StringBuffer subname = new StringBuffer("000best.bin");
            //DataInputStream fptr;
            //SubPopulation net;

            //for( int l = 0; l < 1000; ++l ) {
            //    char j = (char)(l/100);
            //    subname.setCharAt(0, (char)(j + 48));
            //    j = (char)(l%100);
            //    subname.setCharAt(1, (char)(j/10 + 48));
            //    subname.setCharAt(2, (char)(j%10 + 48));
            //    try {
            //        fptr = new DataInputStream( new FileInputStream( subname.toString() ) );
            //        fptr.close();
            //        net = new SubPopulation(subname.toString());
            //        fitness = 0;

            //        // evaluate network
            //        //ccfitness += state.apply(net);
            //        if( fitness > best) {
            //            best = fitness;
            //        }
            //        PrintWriter pw = new PrintWriter( new FileOutputStream( "report.txt", true ) );
            //        pw.write(l+" ");
            //        // Aluminum stuff 
            //        //  fprintf(fptr, "%f %f",
            //        //      ((totdeltasum + deltamaara) / nsulatus) / (MAXSUBS + 1),
            //        //     deltamaara / nsulatus);
            //        //for( j = 0; j < MAXSUBS; j++)
            //        //	fprintf(fptr, " %f", deltasum[j] / nsulatus);
            //        //fprintf(fptr, "\n");

            //        pw.close();
            //    } catch( IOException e ) {
            //        Console.WriteLine(e);
            //    }
            //}

            //try {
            //    PrintWriter pw = new PrintWriter( new FileOutputStream( "analyze.out", true ) );
            //    pw.println(1/best);
            //    pw.close();
            //} catch( IOException e ) {
            //    Console.WriteLine(e);
            //}
            //Console.WriteLine(1/best);		

            throw new NotImplementedException();
        }

        //----------------------------------------------------------------------
        //
        public void loadSeedNet(String filename)
        {
            netType = SECONDORDER; //change this to read the type from the filename;
            subPopulationCount = bestNetwork.load(filename);
            Console.WriteLine("NUM_POPS " + subPopulationCount);
            Console.WriteLine("GENE_SIZE " + bestNetwork.getGeneSize());
        }

        //----------------------------------------------------------------------
        // add a new subbpop. In effect: add a unit to the networks.
        public void addSubPop()
        {
            SubPopulation newSp; //pointer to new subpop.
            addConnections();

            for (int i = 0; i < subpops.Length; ++i)
            {
                //freeze best network? false=yes
                subpops[i].evolvable = true; // true = no.
            }
            ++subPopulationCount; // inc # subpops
            //		setupNetDimensions();  //adjust NUM_INPUTS, GENE_SIZE
            phaseBest.addNeuron(); // add a neuron to the phasebest and best net.
            //  phaseBest.fitness = 0;
            bestNetwork.addNeuron();
            newSp = new SubPopulation(subPopulationSize, bestNetwork); //constuct and create
            newSp.create(); //  new subpop

            //		subpops.add( newSp );		// put its pointer in EspAlgorithm's vector of sp pointers.
            var temp = new SubPopulation[subpops.Length + 1];
            Array.Copy(subpops, 0, temp, 0, subpops.Length);
            temp[temp.Length - 1] = newSp;
            subpops = temp;

            Console.WriteLine("Adding SUBPOP " + (subPopulationCount - 1));
        }

        public void endEvolution()
        {
            printStats();

            Console.WriteLine("Final fitness  = " + currentBest.fitness);

            Console.WriteLine("BYE!");
        }


        //	private EspAlgorithm(final EspAlgorithm) // just for safety
        //	private EspAlgorithm(final EspAlgorithm)

        //////////////////////////////////////////////////////////////////////
        //
        //  evaluation stage.  Create numTrials networks, each containing
        //  ZETA neurons selected randomly from the population.  Each network
        //  is evaluated and each participating neuron receives the fitness
        //  evaluations of each network it parcipates in.
        //
        //////////////////////////////////////////////////////////////////////

        //--------------------------------------------------------------------
        // output a new network of the appropriate type
        private Network genNetType()
        {
            Network rc = null;
            switch (netType)
            {
                case FEEDFORWARD:
                {
                    rc = (new FeedForwardNetwork(environ.getInputSize(), subPopulationCount, environ.getOutputSize()));
                }
                    break;
                case FULLYRECURRENT:
                {
                    rc =
                        (new FullyRecurrentNetwork(environ.getInputSize(), subPopulationCount, environ.getOutputSize()));
                }
                    break;
                case SIMPLERECURRENT:
                {
                    rc =
                        (new SimpleRecurrentNetwork(environ.getInputSize(), subPopulationCount, environ.getOutputSize()));
                }
                    break;
                case SECONDORDER:
                {
                    rc =
                        (new SecondOrderRecurrentNetwork(environ.getInputSize(), subPopulationCount,
                            environ.getOutputSize()));
                }
                    break;
                default:
                {
                    rc = (new FullyRecurrentNetwork(0, 0, 0));
                }
                    break;
            }
            return rc;
        }

        //----------------------------------------------------------------------
        // reset fitness vals
        private void evalReset()
        {
            for (int i = 0; i < subpops.Length; ++i)
            {
                subpops[i].evalReset();
            }
        }

        //--------------------------------------------------------------------
        // evaluate the networks on the task

        private void performEval()
        {
            Network net = genNetType();
            Network bestNet = genNetType();
            //bestNet.create();
            for (int i = 0; i < numTrials; ++i)
            {
                // find random subpopulation
                net.setRandomNetwork(subpops);
                net.incrementTests();

                ++evaluations;
                //evaluate the network
                net.fitness = environ.evalNet(net);

                //net.printWeight(ALL_NETS);

                //    Console.WriteLine("fit %d  %f",i, net.fitness);
                if (MIN)
                {
                    if (net.fitness < bestNet.fitness)
                    {
                        bestNet = (Network) net.Clone();
                    }
                }
                else if (net.fitness > bestNet.fitness)
                {
                    bestNet = (Network) net.Clone();
                }

                net.addFitness(); // add network fitness to its neurons 
            }
            if (MIN)
            {
                if (bestNet.fitness < phaseBest.fitness)
                {
                    phaseBest = (Network) bestNet.Clone();
                }
            }
            else if (bestNet.fitness > phaseBest.fitness)
            {
                phaseBest = (Network) bestNet.Clone();
            }

            perfQ.Insert(0, phaseBest.fitness);

            if (MIN)
            {
                if (bestNet.fitness <= environ.getMaxFitness())
                {
                    phaseBest.fitness = 1000000;
                    perfQ.Clear();
                    if (environ.hasNextTask())
                    {
                        environ.nextTask();
                    }
                    else
                    {
                        finished = true;
                        currentBest = bestNet;
                        return;
                    }
                }
            }
            else if (bestNet.fitness >= environ.getMaxFitness())
            {
                phaseBest.fitness = 0.0;
                perfQ.Clear();
                if (environ.hasNextTask())
                {
                    environ.nextTask();
                }
                else
                {
                    finished = true;
                    currentBest = bestNet;
                    return;
                }
            }

            // if performance stagnates, do something
            if (perfQ.Count >= STAG)
            {
                if (MIN)
                {
                    if (bestNet.fitness >= perfQ[STAG - 1])
                    {
                        handleStagnation();
                    }
                }
                else if (bestNet.fitness <= perfQ[STAG - 1])
                {
                    handleStagnation();
                }
            }

            //Console.WriteLine("%f %f" + perfQ.front() + perfQ[STAG-1] );
            Console.WriteLine("gen " + generation + ": best " + bestNet.fitness + ", task best " + phaseBest.fitness 
                //+ ", overall best " + bestNetwork.fitness 
                );

            currentBest = bestNet;
        }

        //----------------------------------------------------------------------
        // get average fitness level.  
        private void average()
        {
            for (int i = 0; i < subpops.Length; ++i)
            {
                subpops[i].average();
            }
        }

        /////////////////////////////////////////////////////////////////////
        //
        // ESP I/O fns
        private void printNeurons()
        {
            String filename = "./" + seed /*getpid()*/+ "_" + subPopulationCount + "_" + generation + ".pop";


            var fptr = new StreamWriter(filename);
            for (int i = 0; i < subpops.Length; ++i)
            {
                subpops[i].printWeight(fptr);
            }
            fptr.Close();
        }

        //----------------------------------------------------------------------
        // make a decision about what to do when performace stagnates
        private void handleStagnation()
        {
            perfQ.Clear();

            if (phaseBest.fitness == bestNetwork.fitness)
            {
                if (removeSubPop(phaseBest))
                {
                    bestNetwork = (Network) phaseBest.Clone();
                    newDeltaPhase();
                }
                else
                    addSubPop();
                environ.simplifyTask();
                phaseBest.fitness = 0.0;
            }
            else
            {
                bestNetwork = (Network) phaseBest.Clone();
                //sprintf(filename, "./NETS/net%lu_%d_%d", getpid(), subpops.Length, generation);
                //nets[b].saveText(filename);
                newDeltaPhase();
            }
        }

        //----------------------------------------------------------------------
        // Evoluate a new delta phase 
        private void newDeltaPhase()
        {
            Console.WriteLine("DELTA started");
            SKIP = true;
            for (int k = 0; k < subpops.Length; ++k)
            {
                subpops[k].deltify(bestNetwork.getNeuron(k));
            }
        }

        //----------------------------------------------------------------------
        // opposite of addSubPop.
        private bool removeSubPop(int sp)
        {
            bool rc = false;
            if (sp >= 0)
            {
                removeConnections(sp);
                --subPopulationCount;
                //    		setupNetDimensions();
                phaseBest.removeNeuron(sp);
                bestNetwork.removeNeuron(sp);
                //    		delete subpops[sp];
                //    		subpops.erase(subpops.begin()+sp);      
                var temp = new SubPopulation[subpops.Length - 1];
                Array.Copy(subpops, 0, temp, 0, sp);
                Array.Copy(subpops, sp + 1, temp, sp, temp.Length - sp);
                subpops = temp;

                Console.WriteLine("Remove SUBPOP " + sp);
                Console.WriteLine("Now " + subpops.Length + " subpops");
                rc = true;
            }
            return rc;
        }

        //----------------------------------------------------------------------
        private bool removeSubPop(Network net)
        {
            int sp = net.lesion(environ); //see which (if any) subpop can be removed
            return (removeSubPop(sp)); // remove it.
        }


        //----------------------------------------------------------------------
        // add the appropriate connection wieghts that are needed to add
        //   a unit to each type of network.
        private void addConnections()
        {
            int i, j;
            switch (netType)
            {
                case FEEDFORWARD:
                    break;
                case FULLYRECURRENT:
                case SIMPLERECURRENT:
                    for (i = 0; i < subpops.Length; ++i) // add connection to neurons in spops
                        subpops[i].addConnection(environ.getInputSize() + subPopulationCount);
                    break;
                case SECONDORDER:
                    for (i = 0; i < subpops.Length; ++i) // add connection to neurons in spops
                        for (j = 1; j < environ.getInputSize() + 1; ++j) // add connection to neurons in spops
                            subpops[i].addConnection(subpops.Length*j + j - 1);
                    break;
            }
        }

        //----------------------------------------------------------------------
        // opposite of addConnections.
        private void removeConnections(int sp)
        {
            int i, j;
            switch (netType)
            {
                case FEEDFORWARD:
                    break;
                case FULLYRECURRENT:
                case SIMPLERECURRENT:
                    for (i = 0; i < subpops.Length; ++i) // remove connection to neurons in spops
                        subpops[i].removeConnection(environ.getInputSize() + subPopulationCount - 1);
                    break;
                case SECONDORDER:
                    for (i = 0; i < subpops.Length; ++i)
                        for (j = 1; j < environ.getInputSize() + 1; ++j) // remove connection to neurons in spops
                            subpops[i].removeConnection((subpops.Length - 1)*j);
                    break;
            }
        }


        private void printStats()
        {
            Console.WriteLine();
            Console.WriteLine("Total number of network evaluations : " + (generation*numTrials));
        }


        private static void parseArgs(String[] args, EspAlgorithm espAlgorithm)
        {
            // Command-line defaults.
            const int DEFAULT_NUM_POPS = 10;
            const int DEFAULT_POP_SIZE = 100;

            // Set defaults.
            espAlgorithm.subPopulationCount = DEFAULT_NUM_POPS;
            espAlgorithm.subPopulationSize = DEFAULT_POP_SIZE;
            espAlgorithm.netType = FEEDFORWARD;
            espAlgorithm.seed = DateTime.Now.Ticks; //getpid();

            for (int argc = 0; argc < args.Length; argc++)
            {
                if (args[argc].Equals("--plot"))
                {
                    PLOTTING = true;
                }
                else if (args[argc].Equals("--delta"))
                {
                    DELTA = true;
                }
                else if (args[argc].Equals("--minimize"))
                {
                    MIN = true;
                }
                else if (args[argc].Equals("--stag"))
                {
                    STAG = Int32.Parse(args[argc + 1]);
                    argc++;
                    Console.WriteLine("STAG : " + STAG);
                }
                else if (args[argc].Equals("--mutation"))
                {
                    MUT_RATE = Double.Parse(args[argc + 1]);
                    argc++;
                    Console.WriteLine("MUT_RATE : " + MUT_RATE);
                }
                else if (args[argc].Equals("-z"))
                {
                    espAlgorithm.subPopulationCount = Int32.Parse(args[argc + 1]);
                    argc++;
                }
                else if (args[argc].Equals("-n"))
                {
                    espAlgorithm.subPopulationSize = Int32.Parse(args[argc + 1]);
                    argc++;
                }
                else if (args[argc].Equals("-s"))
                {
                    espAlgorithm.seed = Int32.Parse(args[argc + 1]);
                    argc++;
                }
                else if (args[argc].Equals("-t"))
                {
                    espAlgorithm.netType = Int32.Parse(args[argc + 1]);
                    argc++;

                    //Additions by Jacob Schrum, August 2007
                }
                else if (args[argc].Equals("--maxGens"))
                {
                    maxGenerations = Int32.Parse(args[argc + 1]);
                    argc++;
                    Console.WriteLine("MAX GENERATIONS : " + maxGenerations);
                }
                else if (args[argc].Equals("-toFile"))
                {
                    try
                    {
                        Console.SetOut(new StreamWriter(args[argc + 1]));
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine("Must provide valid filename.");
                        Console.WriteLine(e);
                    }
                    argc++;
                    //End Additions by Jacob Schrum
                }
                else if (args[argc].Equals("-x"))
                {
                    analyze = true;
                }
                else if (args[argc].Equals("-f"))
                {
                    espAlgorithm.seedNet = Int32.Parse(args[argc + 1]);
                    argc++;
                }
                else if (args[argc].Equals("-h"))
                {
                    Console.WriteLine("Usage: " + args[0] + " <options>");
                    Console.WriteLine("options:");
                    Console.WriteLine("  -z Z : Z number of hidden units.");
                    Console.WriteLine("  -n N : N neurons in each subpop");
                    Console.WriteLine("  -s S : Initial integer seed S.");
                    Console.WriteLine("  -t T : Type of Network.");
                    Console.WriteLine("         0 = Feed Forward, 1 = Simple Recurrent, 2 = 2ndOrder Recurrent");
                    Console.WriteLine("         3 = Fully Recurrent");
                    //Console.WriteLine( "  -f F : Seed network. ");
                    Console.WriteLine("  --mutation M : Mutation rate in percent (0,1). ");
                    Console.WriteLine("  --stag STAG : Stagnation threshold, number of generations. ");
                    Console.WriteLine("  --minimize : Minimize fitness. ");
                    //      Console.WriteLine( "  -x   : analyze nets.");

                    //Additions by Jacob Schrum, August 2007
                    Console.WriteLine("  --maxGens MG : Maximum generations to run for. ");
                    Console.WriteLine("  --toFile FN : Output to file FN instead of console. ");
                    //End Additions by Jacob Schrum

                    Console.WriteLine("  -h   : this message.");
                    Console.WriteLine(
                        "Default: " + args[0]
                        + " -z " + DEFAULT_NUM_POPS
                        + " -n " + DEFAULT_POP_SIZE
                        + " -s getpid()"
                        + " -t " + FEEDFORWARD
                        + " --mutation 0.4"
                        + " --stag 20"
                        + " --maxGens " + maxGenerations);
                    Console.WriteLine();
                    // System.exit(0);
                }
            }
        }

        private static void checkArgs(int subPopulationCount, int popSize)
        {
            if (MIN)
            {
                Console.WriteLine("MINIMIZING");
            }
        }

        public Network Evoluate(IEnvironment environment)
        {
            if (Logger != null)
            {
                Console.SetOut(new LoggerWriter(Logger));
            }

            parseArgs(new string[] {}, this);
            checkArgs(this.subPopulationCount, this.subPopulationSize);

            init(environment);

            //		EspAlgorithm EspAlgorithm = new EspAlgorithm(subPopulationCount, subPopulationCount, cartpole, netType); //construct the ESP.

            if (analyze)
            {
                findChampion();
                // System.exit(1);
            }

            create(); // create the neurons.
            Network network = evolve(maxGenerations); // evolve them.
            return network;
        }

        //public static void main(String[] args)
        //{
        //    // Parse command-line arguments.
        //    int seedNet = 0;
        //    int netType;

        //    SignalHandler.setUpCtrlC();
        //    EspAlgorithm EspAlgorithm = new EspAlgorithm(); //construct the ESP.
        //    parseArgs(args, EspAlgorithm);
        //    checkArgs(EspAlgorithm.subPopulationCount, EspAlgorithm.subPopulationSize);


        //    IEnvironment environment = new MFCCEnvironment();


        //    EspAlgorithm.init(environment);

        //    //		EspAlgorithm EspAlgorithm = new EspAlgorithm(subPopulationCount, subPopulationCount, cartpole, netType); //construct the ESP.

        //    if (analyze)
        //    {
        //        EspAlgorithm.findChampion();
        //        // System.exit(1);
        //    }
        //    EspAlgorithm.create(); // create the neurons.
        //    EspAlgorithm.evolve(maxGenerations); // evolve them.
        //}
    }

    public class LoggerWriter : TextWriter
    {
        private readonly Action<string> logger;

        public LoggerWriter(Action<string> logger)
        {
            this.logger = logger;
        }

        public override Encoding Encoding
        {
            get { return Encoding.UTF8; }
        }

        public override void WriteLine(string str)
        {
            logger(str);
        }
    }
}